#include "bsp_can.h"
#include "stm32f1xx_hal.h"
#include "stdio.h"
#include "can.h"

CAN_TxHeaderTypeDef   TxHeader;
CAN_RxHeaderTypeDef   RxHeader;
uint8_t               TxData[8]={0};
uint8_t               RxData[8]={0};
uint32_t							TxMailBox;
moto_measure_t motor[4];

#define ABS(x)	( (x>0) ? (x) : (-x) )

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* _hcan)
{
	__HAL_CAN_ENABLE_IT(&hcan,CAN_IT_RX_FIFO0_MSG_PENDING);
 	  if (HAL_CAN_GetRxMessage(&hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
  {
    /* Reception Error */
    Error_Handler();
  }
	
	switch(RxHeader.StdId)
	{
		case 0x201:
			{
				static uint8_t i;
				i = RxHeader.StdId - 0x201;
				motor[i].msg_cnt++ <= 50	?	get_moto_offset(&motor[i], RxData) : get_moto_measure(&motor[i], RxData );
				get_moto_measure(&motor[i], RxData);
				break;
			}
	  case 0x202:
			{
				static uint8_t j;
				j = RxHeader.StdId - 0x201;
				motor[j].msg_cnt++ <= 50	?	get_moto_offset(&motor[j], RxData) : get_moto_measure(&motor[j], RxData );
				get_moto_measure(&motor[j], RxData);
				break;
			}			
		
		case 0x205:
			{
				static uint8_t k;
				k = RxHeader.StdId - 0x205+2;
				motor[k].msg_cnt++ <= 50	?	get_moto_offset(&motor[k], RxData) : get_moto_measure(&motor[k], RxData );
				get_moto_measure(&motor[k], RxData);
				break;
			}
		case 0x206:
			{
				static uint8_t m;
				m = RxHeader.StdId - 0x205+2;;
				motor[m].msg_cnt++ <= 50	?	get_moto_offset(&motor[m], RxData) : get_moto_measure(&motor[m], RxData );
				get_moto_measure(&motor[m], RxData);
				break;
			}
	}

}

void get_moto_measure(moto_measure_t *ptr, uint8_t Data[8])
{

	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(Data[0]<<8 | Data[1]) ;
	ptr->real_current  = (int16_t)(Data[2]<<8 | Data[3]);
	ptr->speed_rpm = ptr->real_current;	
	ptr->given_current = (int16_t)(Data[4]<<8 | Data[5])/-5;
	ptr->hall = Data[6];
	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
}

void get_moto_offset(moto_measure_t *ptr, uint8_t Data[8])
{
	ptr->angle = (uint16_t)(Data[0]<<8 | Data[1]) ;
	ptr->offset_angle = ptr->angle;
}



void get_total_angle(moto_measure_t *p)
{
		int res1, res2, delta;
	if(p->angle < p->last_angle){			
		res1 = p->angle + 8192 - p->last_angle;	
		res2 = p->angle - p->last_angle;				
	}else{	
		res1 = p->angle - 8192 - p->last_angle ;
		res2 = p->angle - p->last_angle;	
	}
	if(ABS(res1)<ABS(res2))
		delta = res1;
	else
		delta = res2;

	p->total_angle += delta;
	p->last_angle = p->angle;
}

void set_moto_current(CAN_HandleTypeDef* _hcan,CAN_TxHeaderTypeDef* TxHeader,uint8_t TxData[8] ,uint32_t*	mailbox,int16_t iq1, int16_t iq2, int16_t iq3, int16_t iq4){

	TxHeader->StdId = 0x200;
	TxHeader->IDE = CAN_ID_STD;
	TxHeader->RTR = CAN_RTR_DATA;
	TxHeader->DLC = 0x08;
	TxData[0] = iq1 >> 8;
	TxData[1] = iq1;
	TxData[2] = iq2 >> 8;
	TxData[3] = iq2;
	TxData[4] = iq3 >> 8;
	TxData[5] = iq3;
	TxData[6] = iq4 >> 8;
	TxData[7] = iq4;
			if (HAL_CAN_AddTxMessage(&hcan, TxHeader,TxData,mailbox) != HAL_OK)
	{
		printf("ERROR\r\n");
	}
else{
}

}

void set_moto_current1(CAN_HandleTypeDef* _hcan,CAN_TxHeaderTypeDef* TxHeader,uint8_t TxData[8] ,uint32_t*	mailbox,int16_t iq1, int16_t iq2, int16_t iq3, int16_t iq4){

	TxHeader->StdId = 0x1FF;;
	TxHeader->IDE = CAN_ID_STD;
	TxHeader->RTR = CAN_RTR_DATA;
	TxHeader->DLC = 0x08;
	TxData[0] = iq1 >> 8;
	TxData[1] = iq1;
	TxData[2] = iq2 >> 8;
	TxData[3] = iq2;
	TxData[4] = iq3 >> 8;
	TxData[5] = iq3;
	TxData[6] = iq4 >> 8;
	TxData[7] = iq4;
			if (HAL_CAN_AddTxMessage(&hcan, TxHeader,TxData,mailbox) != HAL_OK)
	{
		printf("ERROR\r\n");
	}
else{
}

}



void reset_motor(moto_measure_t* motor)
{
motor->round_cnt=0;
motor->last_angle=0;
motor->real_current=0;
}
